#include <fstream>
#include <iostream>
#include <eigen3/Eigen/Dense>

#include "motion/semintf.h"
#include "zucmodule_comm.h"

#define PI 3.141592653

static int datanum = 5000;
static double data[5000][9] = {0};
static double carpose[5000][3] = {0};
static double tfvalue[5000][CART_DIM] = {0};

void convertWorldPointToUser(const CartesianPos* base_point, const ZucPose* current_user_offset, CartesianPos* user_point)
{
    PmRpy base_point_rpy;
    PmRotationMatrix base_point_matrix;
    base_point_rpy.y = base_point->rpy.y * PM_PI / 180.0;
    base_point_rpy.p = base_point->rpy.p * PM_PI / 180.0;
    base_point_rpy.r = base_point->rpy.r * PM_PI / 180.0;
    pmRpyMatConvert(&base_point_rpy, &base_point_matrix);

    PmRpy user_offset_rpy;
    PmRotationMatrix user_offset_matrix;
    user_offset_rpy.y = current_user_offset->a * PM_PI / 180.0;
    user_offset_rpy.p = current_user_offset->b * PM_PI / 180.0;
    user_offset_rpy.r = current_user_offset->c * PM_PI / 180.0;
    pmRpyMatConvert(&user_offset_rpy, &user_offset_matrix);

    PmHomogeneous base_point_homogeneous;
    base_point_homogeneous.rot = base_point_matrix;
    base_point_homogeneous.tran = base_point->point;

    PmHomogeneous user_offset_homogeneous;
    user_offset_homogeneous.rot = user_offset_matrix;
    user_offset_homogeneous.tran = current_user_offset->tran;

    PmHomogeneous user_offset_homogeneous_inverse;
    pmHomInv(&user_offset_homogeneous, &user_offset_homogeneous_inverse);

    PmHomogeneous result_homogeneous;
    pmHomHomMult(&user_offset_homogeneous_inverse, &base_point_homogeneous, &result_homogeneous);

    PmRpy result_base_rpy;
    pmMatRpyConvert(&result_homogeneous.rot, &result_base_rpy);

    user_point->point = result_homogeneous.tran;
    user_point->rpy.y = result_base_rpy.y * 180.0 / PM_PI;
    user_point->rpy.p = result_base_rpy.p * 180.0 / PM_PI;
    user_point->rpy.r = result_base_rpy.r * 180.0 / PM_PI;
}

void convertUserPointToWorld(const CartesianPos* user_point, const ZucPose* current_user_offset, CartesianPos* base_point)
{
    PmRpy user_offset_rpy;
    PmRotationMatrix user_offset_matrix;
    user_offset_rpy.y = current_user_offset->a * PM_PI / 180.0;
    user_offset_rpy.p = current_user_offset->b * PM_PI / 180.0;
    user_offset_rpy.r = current_user_offset->c * PM_PI / 180.0;
    pmRpyMatConvert(&user_offset_rpy, &user_offset_matrix);

    PmRpy user_point_rpy;
    PmRotationMatrix user_point_matrix;
    user_point_rpy.y = user_point->rpy.y * PM_PI / 180.0;
    user_point_rpy.p = user_point->rpy.p * PM_PI / 180.0;
    user_point_rpy.r = user_point->rpy.r * PM_PI / 180.0;
    pmRpyMatConvert(&user_point_rpy, &user_point_matrix);

    PmHomogeneous user_point_homogeneous;
    user_point_homogeneous.rot = user_point_matrix;
    user_point_homogeneous.tran = user_point->point;

    PmHomogeneous offset_homogeneous;
    offset_homogeneous.rot = user_offset_matrix;
    offset_homogeneous.tran = current_user_offset->tran;

    PmHomogeneous result_homogeneous;
    pmHomHomMult(&offset_homogeneous, &user_point_homogeneous, &result_homogeneous);

    PmRpy current_base_rpy;
    pmMatRpyConvert(&result_homogeneous.rot, &current_base_rpy);

    base_point->point = result_homogeneous.tran;
    base_point->rpy.y = current_base_rpy.y * 180.0 / PM_PI;
    base_point->rpy.p = current_base_rpy.p * 180.0 / PM_PI;
    base_point->rpy.r = current_base_rpy.r * 180.0 / PM_PI;
}

void CalculateRotateMatirx(const double* cartPos, double rotate_matrix[3][3])
{
    double rx = cartPos[0];
    double ry = cartPos[1];
    double rz = cartPos[2];
    // T = rotz(rz)roty(ry)rotx(rx)
    rotate_matrix[0][0] = cos(ry) * cos(rz);
    rotate_matrix[0][1] = -cos(rx) * sin(rz) + sin(rx) * sin(ry) * cos(rz);
    rotate_matrix[0][2] = sin(rx) * sin(rz) + cos(rx) * sin(ry) * cos(rz);
    rotate_matrix[1][0] = cos(ry) * sin(rz);
    rotate_matrix[1][1] = cos(rx) * cos(rz) + sin(rx) * sin(ry) * sin(rz);
    rotate_matrix[1][2] = -sin(rx) * cos(rz) + cos(rx) * sin(ry) * sin(rz);
    rotate_matrix[2][0] = -sin(ry);
    rotate_matrix[2][1] = sin(rx) * cos(ry);
    rotate_matrix[2][2] = cos(rx) * cos(ry);
}

void CalculatePayloadCenter(const double sensordata[][CART_DIM], double* MassCenter)
{
    //Calculate payload center
    Eigen::VectorXd M(3 * datanum);
    Eigen::MatrixXd M_matrix(3 * datanum, CART_DIM);
    Eigen::VectorXd payloadcenter(CART_DIM);
    for (int i = 0; i < datanum; i++)
    {
        M(3 * i) = sensordata[i][3];
        M(3 * i + 1) = sensordata[i][4];
        M(3 * i + 2) = sensordata[i][5];
        M_matrix(3 * i, 0) = 0.0;
        M_matrix(3 * i, 1) = sensordata[i][2];
        M_matrix(3 * i, 2) = -sensordata[i][1];
        M_matrix(3 * i + 1, 0) = -sensordata[i][2];
        M_matrix(3 * i + 1, 1) = 0.0;
        M_matrix(3 * i + 1, 2) = sensordata[i][0];
        M_matrix(3 * i + 2, 0) = sensordata[i][1];
        M_matrix(3 * i + 2, 1) = -sensordata[i][0];
        M_matrix(3 * i + 2, 2) = 0.0;
        M_matrix(3 * i, 3) = 1.0;
        M_matrix(3 * i, 4) = 0.0;
        M_matrix(3 * i, 5) = 0.0;
        M_matrix(3 * i + 1, 3) = 0.0;
        M_matrix(3 * i + 1, 4) = 1.0;
        M_matrix(3 * i + 1, 5) = 0.0;
        M_matrix(3 * i + 2, 3) = 0.0;
        M_matrix(3 * i + 2, 4) = 0.0;
        M_matrix(3 * i + 2, 5) = 1.0;
    }
    payloadcenter = ((M_matrix.transpose() * M_matrix).inverse()) * M_matrix.transpose() * M;
    //unit translation m -> mm
    MassCenter[0] = payloadcenter(0) * 1000;
    MassCenter[1] = payloadcenter(1) * 1000;
    MassCenter[2] = payloadcenter(2) * 1000;
}

void CalculatePayloadMass(const double cartpose[][3], const double sensordata[][CART_DIM], double* Mass)
{
    //Calculate payload mass
    Eigen::VectorXd F(3 * datanum);
    Eigen::MatrixXd F_matrix(3 * datanum, CART_DIM);
    Eigen::VectorXd payloadmass(CART_DIM);
    double Rot_temp[3][3];
    for (int i = 0; i < datanum; i++)
    {
        F(3 * i) = sensordata[i][0];
        F(3 * i + 1) = sensordata[i][1];
        F(3 * i + 2) = sensordata[i][2];
        CalculateRotateMatirx(cartpose[i], Rot_temp);
        F_matrix(3 * i, 0) = Rot_temp[0][0];
        F_matrix(3 * i, 1) = Rot_temp[1][0];
        F_matrix(3 * i, 2) = Rot_temp[2][0];
        F_matrix(3 * i + 1, 0) = Rot_temp[0][1];
        F_matrix(3 * i + 1, 1) = Rot_temp[1][1];
        F_matrix(3 * i + 1, 2) = Rot_temp[2][1];
        F_matrix(3 * i + 2, 0) = Rot_temp[0][2];
        F_matrix(3 * i + 2, 1) = Rot_temp[1][2];
        F_matrix(3 * i + 2, 2) = Rot_temp[2][2];
        F_matrix(3 * i, 3) = 1.0;
        F_matrix(3 * i, 4) = 0.0;
        F_matrix(3 * i, 5) = 0.0;
        F_matrix(3 * i + 1, 3) = 0.0;
        F_matrix(3 * i + 1, 4) = 1.0;
        F_matrix(3 * i + 1, 5) = 0.0;
        F_matrix(3 * i + 2, 3) = 0.0;
        F_matrix(3 * i + 2, 4) = 0.0;
        F_matrix(3 * i + 2, 5) = 1.0;
    }
    payloadmass = ((F_matrix.transpose() * F_matrix).inverse()) * F_matrix.transpose() * F;
    *Mass = sqrt(payloadmass(0) * payloadmass(0) + payloadmass(1) * payloadmass(1) + payloadmass(2) * payloadmass(2)) / 9.81;
}

void GetPayLoad(double* PayLoad)
{
    std::ifstream infile;
    infile.open("/home/jakauser/tooldata.txt");
    if (!infile.is_open())
    {
        std::cout << "Error opening file";
        exit(1);
    }
    memset(data, 0, sizeof(data[0][0]) * 5000 * 9);
    double* ptr = &data[0][0];
    while (!infile.eof())
    {
        infile >> *ptr;
        ptr++;
    }
    infile.close();

    for (int i = 0; i < 5000; i++)
    {
        if (data[i][0] == 0 && data[i][3] == 0)
        {
            datanum = i;
            break;
        }
    }

    for (int j = 0; j < datanum; j++)
    {
        carpose[j][0] = data[j][0] / 180 * PI;
        carpose[j][1] = data[j][1] / 180 * PI;
        carpose[j][2] = data[j][2] / 180 * PI;
        tfvalue[j][0] = data[j][3];
        tfvalue[j][1] = data[j][4];
        tfvalue[j][2] = data[j][5];
        tfvalue[j][3] = data[j][6];
        tfvalue[j][4] = data[j][7];
        tfvalue[j][5] = data[j][8];
    }

    double MassCenter[3] = {0};
    double Mass = 0.0;

    CalculatePayloadCenter(tfvalue, MassCenter);
    CalculatePayloadMass(carpose, tfvalue, &Mass);
    // std::cout << "masscenter: " << MassCenter[0] << " " << MassCenter[1] << " " << MassCenter[2]<< std::endl;
    // std::cout << "mass: " << Mass << std::endl;
    // std::cout << "datanum: " << datanum << std::endl;
    for (int i = 0; i < 3; i++) { PayLoad[i] = MassCenter[i]; }
    PayLoad[3] = Mass;
}

//create a semaphore
int sem_get(key_t key)
{
    int sem_id;
    sem_id = semget(key, 1, 0666 | IPC_CREAT);
    union semun sem_union;
    sem_union.val = 1;
    if (semctl(sem_id, 0, SETVAL, sem_union) == -1)
    {
        return -1;
    }
    return sem_id;
}

//obtain a semaphore
int sem_p(int sem_id)
{
    struct sembuf sem_buf;
    sem_buf.sem_op = -1;
    sem_buf.sem_flg = SEM_UNDO;
    sem_buf.sem_num = 0;
    if (semop(sem_id, &sem_buf, 1) == -1)
    {
        return -1;
    }
    return 0;
}

//release an semaphore
int sem_v(int sem_id)
{
    struct sembuf sem_buf;
    sem_buf.sem_flg = SEM_UNDO;
    sem_buf.sem_num = 0;
    sem_buf.sem_op = 1;
    if (semop(sem_id, &sem_buf, 1) == -1)
    {
        return -1;
    }
    return 0;
}
